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ABSTRACT 


The  accurate  determination  of  spacecraft  attitude  has  always  been  a  critical  issue  in 
many  applications.  The  presence  of  imperfect  sensors  introduces  errors  in  the  system  and  affects 
the  outcome  of  the  mission.  One  of  the  most  significant  sensors  is  the  rate  gyroscope.  Particu¬ 
larly,  the  rate  gyros  are  known  to  degrade  with  time,  introducing  random  noise  and  bias.  This 
calls  for  estimation  algorithms  which  process  the  measured  data  in  order  to  reduce  the  effects  of 
the  disturbances  to  a  minimum.  This  research  presents  an  approach  which  takes  full  advantage 
on  the  nonlinear  dynamics  and  possibly  non-Gaussian  disturbances.  It  is  based  on  recent  work 
involving  particle  filters,  where  the  probability  density  functions  are  approximated  by  a  rela¬ 
tively  large  number  of  parameters.  It  is  shown  that  accurate  attitude  estimation  can  be  obtained 
with  a  manageable  number  of  particles. 
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EXECUTIVE  SUMMARY 


The  great  importance  of  military  and  civilian  space  systems  has  driven  the  need  for  ex¬ 
tending  their  expected  lifetime  and  increasing  the  associated  components’  accuracy.  The  rate 
gyroscope  is  one  of  the  vital  sensors  on  a  spacecraft  and  its  performance  dramatically  affects 
both  lifetime  and  performance.  The  reduction  of  the  rate  gyro  noise  level  has  been  an  issue  for 
quite  a  long  time. 

In  order  to  mitigate  the  undesirable  effects  of  bias  and  measurement  noise  in  a  rate  gyro¬ 
scope,  various  techniques  have  been  developed.  This  thesis  investigates  how  a  new  approach 
can  be  utilized  in  order  to  achieve  better  accuracy  in  a  strongly  nonlinear  system  such  as  a 
spacecraft  attitude  control  system. 

At  first,  the  particle  filtering  technique  is  discussed  and  compared  to  traditional  tech¬ 
niques  such  as  Kalman  filtering  and  extended  Kalman  filtering.  Based  on  the  kind  of  model  and 
the  type  of  the  disturbances,  the  expected  perfonnance  of  each  approach  is  analyzed.  Initially,  in 
order  to  understand  the  proposed  algorithm,  particle  filter  techniques  and  the  Kalman  filter  are 
compared  using  a  simple  dynamic  model.  Then  the  results  for  one-axis  rotation  of  the  spacecraft 
simulator  are  presented  in  terms  of  attitude  estimation. 

The  first  model  of  interest  is  a  linear  model  in  Gaussian  environment.  The  particle  filter 
is  compared  to  the  Kalman  one  which,  as  expected,  is  shown  to  be  the  best  estimator  for  this 
specific  case.  Through  a  series  of  simulations,  the  number  of  required  particles  is  inferred. 

The  second  model  is  the  same  linear  one,  but  in  additive  non-Gaussian  noise.  In  this 
case  the  Kalman  filter  can  not  guarantee  convergence,  so  other  techniques  must  be  developed. 
The  scope  of  the  associated  simulations  is  to  show  the  particle  filter  robustness  when  enough 
particles  are  propagated.  On  the  other  hand,  when  the  number  of  particles  is  insufficient,  large 
errors  occur  in  the  estimation  procedure. 

Finally,  the  particle  filter  is  evaluated  for  the  specific  application  of  spacecraft  attitude 
detennination  using  a  quaternion-based  representation  of  one-axis  rotation  when  Gaussian  noise 
ia  added  in  both  the  state  and  measurement  equations  of  the  developed  nonlinear  model.  The 


xv 


parameters  were  chosen  in  a  way  that  the  model  included  discontinuities  that  had  to  be  over¬ 
come  by  the  proposed  estimator.  The  extended  Kalman  filter  cannot  provide  a  direct  solution  to 
the  problem  since  it  is  based  on  local  linearization  of  the  nonlinear  model,  which  can  only  be 
performed  in  continuous  processes. 

The  particle  filter  proved  to  be  an  excellent  estimator  for  this  particular  application.  The 
objective  was  to  minimize  the  mean-square  error  of  the  measured  roll  angle.  The  results  showed 
that  a  particle  filter  using  2000  particles  can  provide  a  significant  improvement  with  respect  to 
the  roll  angle  measurement  error.  This  conclusion,  in  conjunction  with  the  increasing  computa¬ 
tional  power  of  today’s  machines,  indicated  that  the  particle  filter  can  be  widely  utilized  in  this 
and  similar  applications. 
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I.  INTRODUCTION 


There  has  been  a  considerable  evolution  in  the  development  of  space  systems  dur¬ 
ing  the  past  few  decades.  One  of  the  key  aspects  is  platform  stabilization  and  navigation. 
The  challenge  is  to  keep  the  space  platform  at  a  specific  orientation  with  respect  to  a 
fixed  inertial  reference  frame.  In  particular,  requirements  can  become  very  strict  in  some 
applications.  For  example,  in  laser  pointing  applications  errors  on  the  order  of  one  mil- 
lidegree  could  result  in  a  target  miss  by  several  kilometers. 

The  general  approach  to  reducing  the  impact  of  such  errors  is  to  combine  the  ob¬ 
servations  from  a  number  of  sensors  which  include  star  trackers  and  gyroscopes  measur¬ 
ing  angular  rates.  The  dynamic  equations  relating  the  measured  quantities  to  the  platfonn 
orientations  are  nonlinear.  The  presence  of  miscellaneous  errors  due  to  various  sensors 
increases  the  complexity  of  the  problem.  This  thesis  investigates  a  new  approach  and 
evaluates  its  effectiveness. 

Kalman  filtering  applications  to  spacecraft  attitude  estimation  were  introduced  in 
the  early  1970s.  Since  then,  many  advanced  algorithms  based  on  the  Kalman  filter  have 
been  developed  and  implemented  with  varying  levels  of  success.  Most  of  them  are  based 
on  local  linearization  of  nonlinear  processes  in  order  to  apply  filtering.  Development  of  a 
purely  nonlinear  technique  that  is  applicable  to  spacecraft  attitude  detennination  and 
overcomes  the  problems  of  singularities  and  discontinuities  is  an  extremely  important 
issue. 

A.  OBJECTIVE 

The  objective  of  this  thesis  was  the  discretization,  implementation  and  evaluation 
of  an  algorithm  for  attitude  determination  that  compensates  for  external  disturbances  in 
the  gyroscopes.  This  algorithm  was  based  on  a  newly  developed  technique  that  fully  ex¬ 
ploits  nonlinearities.  In  particular,  an  approach  based  on  particle  filtering  was  evaluated 
in  various  cases.  Particle  filter  applications  have  already  been  developed  in  fields  such  as 
wireless  communications,  target  tracking  and  navigation  systems.  Some  of  them  can  be 
found  in  the  list  of  references. 
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B.  THESIS  OUTLINE 

The  thesis  starts  with  an  overview  of  the  various  aspects  of  the  estimation  prob¬ 
lem,  and  existing  nonlinear  algorithms  are  analyzed  briefly.  Different  variations  of  the 
particle  filter  are  presented  along  with  background  infonnation  on  rigid  body  kinematics. 
Next,  dynamic  modeling  of  a  rate  gyro  and  the  related  algorithm  development  are  dis¬ 
cussed.  Finally,  the  simulation  results  are  presented  and  summarized. 

In  particular,  the  thesis  is  organized  as  follows: 

Chapter  II  provides  a  survey  on  the  existing  filtering  algorithms,  the  particle  filter 
and  its  application  to  rigid  body  kinematics.  The  parameters  used  in  this  thesis  are  de¬ 
fined  and  explained  in  this  chapter,  as  well. 

Chapter  III  describes  the  target  system,  which  is  the  NPS  Three-Axis  Spacecraft 
Simulator  (TASS).  The  development  of  the  proposed  dynamic  model  and  its  discrete  time 
implementation  for  this  system  is  also  presented  here. 

Chapter  IV  presents  the  performance  analysis  and  evaluation  of  the  particle  filter 
algorithm  for  all  cases  that  it  was  tested. 

Chapter  V  summarizes  the  conclusions  and  provides  recommendations  for  the  fu¬ 
ture  research. 
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II.  BACKGROUND  INFORMATION 


In  this  chapter,  the  theoretical  background  for  this  thesis  is  presented  to  the 
reader.  In  particular,  the  general  estimation  problem  is  established  and  the  associated  es¬ 
timation  techniques  are  analyzed,  emphasizing  on  the  particle  filter.  Finally,  an  introduc¬ 
tory  discussion  on  the  rigid  body  attitude  representation  concludes  the  chapter. 

A.  THE  GENERAL  ESTIMATION  PROBLEM 

In  a  number  of  applications  we  need  to  estimate  a  particular  state  of  the  system, 
based  on  observations  from  sensors.  A  mathematical  model  relates  the  state  dynamics  to 
the  measured  signals  and  the  measurement  errors.  This  model,  also  called  the  dynamic 
model,  can  be  completely  described  by  the  following  set  of  equations: 


x(t)  =  f[x(t)]  +  w(t) 

(1) 

y(0  =  g[40]  +  v(0 

(2) 

where  x(t)  is  the  state  to  be  estimated,  y{t)  is  the  available  set  of  measurements,/ and  g 
are  functions  related  to  the  state,  w(t)  is  the  state  noise  and  v(7)  is  the  measurement  noise. 
In  this  general  case,  the  functions/ and  g  can  be  of  any  form  and  the  statistics  of  the  noise 
signals  are  assumed  to  be  known.  The  state  noise  signal  w{t)  is  a  measure  of  the  accuracy 
for  the  dynamic  model,  while  the  measurement  noise  signal  v(t)  is  directly  related  to  the 
specifications  of  the  sensors.  A  basic  assumption  for  the  model  is  that  these  two  noise 
signals  are  uncorrelated.  Equation  (1)  is  called  the  state  equation  and  presents  the  evolu¬ 
tion  of  the  state  in  time,  while  Eq.  (2)  is  called  the  measurement  or  observation  equation 
and  is  the  one  that  relates  the  state  to  the  measurement. 

The  model  described  above  is  a  continuous  time  one.  However,  since  most  of  the 
applications  are  based  on  sampled  data,  discrete  time  models  are  widely  used.  This  type 
of  model  can  be  completely  described  by  the  following  set  of  equations: 


XM  =  f[Xk\  +  Wk 

(3) 

yk  =  g\xk]+vk 

(4) 
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where  x  is  the  state  to  be  estimated,  y  is  the  available  set  of  measurements,/ and  g  are 
functions  related  to  the  state,  w  and  v  are  uncorrelated  noise  sequences,  and  finally  k  is 
the  index  or  iteration  number.  In  this  discrete  time  model,  the  state  equation  represents 
the  evolution  of  the  state  vector  while  the  measurement  equation  relates  the  state  to  the 
measurement  at  each  sample  point. 

In  each  application  the  estimation  problem  consists  of  two  basic  stages,  mathe¬ 
matical  modeling  and  state  estimation.  Mathematical  modeling  includes  the  derivation  of 
the  functions/ and  g  based  on  physical  considerations.  The  statistics  of  the  noise,  as¬ 
sumed  Gaussian  for  convenience,  are  in  general  difficult  to  determine  and  greatly  influ¬ 
ence  the  performance  of  the  estimator.  The  estimation  stage  includes  an  algorithm  that 
provides  the  best  estimate  of  the  state  given  a  set  of  measurements  and  any  initial  condi¬ 
tions  and  constraints  that  apply. 

The  general  estimation  problem  can  be  divided  in  two  categories,  linear  and 
nonlinear.  Also,  with  respect  to  the  statistics  of  the  comprised  noise,  discrimination  be¬ 
tween  Gaussian  and  non-Gaussian  noise  is  useful. 


Due  to  the  difficulty  of  the  general  problem,  most  applications  have  been  based 
on  simplifying  assumptions.  The  easiest  approach  is  based  on  a  linear  model  and  Gaus¬ 
sian  disturbances.  This  leads  to  the  utilization  of  the  Kalman  filter  introduced  in  the  next 
section. 

B.  THE  LINEAR  CASE  IN  GAUSSIAN  ENVIRONMENT 
1.  System  Modeling 

A  significant  number  of  tracking  applications  can  be  implemented  using  linear 
models  in  a  Gaussian  environment.  This  specific  type  of  model  can  be  described  by  the 
following  set  of  equations  in  discrete  time: 


Xk+l  =  FkXk+Wk 


(5) 


34  =  Hkxk  +  vk 


(6) 


where  Fk  is  the  matrix  relating  xk  to  xk+l  in  the  absence  of  a  forcing  function  (similar  to 
the  state  transition  matrix  for  continuous  time  processes),  Hk  is  the  matrix  corresponding 
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to  the  relationship  between  the  measurement  and  the  state  vector,  wk  and  vk  are  white 
Gaussian  uncorrelated  noise  sequences  with  covariance  Qk  and  Rk ,  respectively,  while 
index  k  refers  to  the  time  step  tk .  Assuming  that  x  is  an  //-dimensional  vector  while  y  is 
an  ///-dimensional  one,  Fk  has  to  be  of  dimension  nxn  and  II  k  of  in  x  n  .  Both  Fk  and 
H k  can  be  time  variant  provided  their  values  are  known  for  each  time  step.  We  can  ob¬ 
serve  that  this  model  is  detennined  by  only  linear  relations.  Also,  noise  sequences  can  be 
fully  described  by  two  values,  mean  and  covariance,  and  they  can  be  time  variant  as  well, 
assuming  that  the  covariance  values  are  known  for  each  time  step.  The  following  equa¬ 
tions  fully  describe  the  statistical  properties  of  the  noise  sequences: 


i{wkwTi}  =  Qk8[k-i\ 

(V) 

E{vkvi}  =  RkS[k~i] 

(8) 

{wkv'  }  =  0  M  i,k eR 

(9) 

where  E{.}  is  the  expected  value  operator,  Qk  and  R,  are  the  covariance  values  of  the 
noise  sequences  wk  and  vk  respectively,  and  8  is  the  Dirac  function. 

2.  The  Kalman  Filter 

A  recursive  algorithm  providing  solution  to  this  kind  of  problems  was  developed 
by  R.E.  Kalman  in  1960  and  it  is  known  as  the  Kalman  filter  [1],  As  it  is  stated  in  the 
previous  chapter,  the  objective  is  to  estimate  the  state  for  each  time  step.  For  this  reason, 
the  state  estimate  at  time  tk  is  denoted  by  xk  and  a  new  variable  called  the  estimation 
error  is  defined  by 

ek=xk~xk •  0°) 

The  error  covariance  matrix  is  now  defined  by 

Pk=E{ekeTk  |y,}  (11) 

where  yk  ={yn  i  =  1  ,...,£}  is  the  vector  containing  all  the  available  measurements  up  to 
time  step  tk . 
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It  is  obvious  that  the  estimation  error  vector  is  ^-dimensional,  so  the  error  covari¬ 
ance  matrix  is  of  dimension  nxn  .  The  problem  now  leads  to  the  minimization  of  ek, 

which  is  equivalent  to  the  minimization  of  the  trace  of  Pk .  The  Kalman  filter  accom¬ 
plishes  this  task  by  following  the  procedure  indicated  in  Fig.  1.  An  overview  of  the  in¬ 
termediate  stages  is  presented  in  the  following  sections. 


Repeat  for  k=0,l,2. 


Figure  1 .  Block  Diagram  of  the  Kalman  Filter 


a.  Initialization 

The  Kalman  filter  is  a  recursive  algorithm  and  it  is  repeated  at  each  time 
step.  Flowever,  initial  estimates  for  the  state  vector  and  the  error  covariance  have  to  be 
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determined.  These  values  are  denoted  by  x0  and  P0  where  the  minus  sign  in  the  super¬ 
script  indicates  that  the  related  measurement  has  not  yet  been  taken  under  consideration. 
If  no  estimate  for  the  initial  value  of  the  state  is  available,  then  it  can  be  assumed  that 


x~  =  0 

p0  ~  Qo- 


(12) 


The  Kalman  filter  is  guaranteed  to  converge  to  the  actual  value  of  the 
state,  no  matter  what  value  of  initial  estimate  is  used.  The  error  of  the  initial  estimate  af¬ 
fects  only  the  number  of  iterations  needed  to  reach  convergence. 
b.  Calculation  of  the  Kalman  Gain 

Knowing  the  posterior  estimate  x~k  =  E{xk  |  y/t_1 }  for  each  time  step  tk , 

the  measurement  at  that  time  has  to  be  taken  into  account.  This  happens  by  taking  advan¬ 
tage  of  the  following  equation: 

x,=(I-KtH,)xt+Kiyi  (13) 


where  /  is  the  identity  matrix  and  the  quantity  Kk  is  the  Kalman  gain.  From  Eq.  (13)  it  is 
obvious  that  the  Kalman  gain  is  a  weighting  factor  for  both  the  measurement  and  a  priori 
estimate,  and  it  is  related  to  the  reliability  of  the  observation.  The  optimal  Kalman  gain  is 
constrained  by  the  minimization  criterion  for  the  trace  of  the  error  covariance  matrix. 

This  leads  to  the  following  equation  that  provides  the  optimal  Kalman  gain: 

K>=PtHl(Htp-HTt+Rky'.  (14) 

c.  Update  Equations 

In  this  stage  the  measurement  is  taken  into  account  and  an  update  to  both 
state  estimation  and  error  covariance  takes  place.  The  optimal  error  covariance  matrix  is 
obtained  using  the  Kalman  gain  computed  in  the  previous  stage.  The  equations  that  drive 
this  stage  are  the  following: 


=  -V.  +Kt  ( V,  -  Htxk  ) 

(15) 

(16) 
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d.  Projection  Equations 

The  update  stage  concludes  the  estimation  for  time  step  tk .  However,  pre¬ 
dictions  for  both  state  vector  and  error  covariance  matrix  have  to  be  generated  in  order  to 
continue  the  recursion  to  the  following  time  step  tk+l .  These  predictions  depend  only  on 

the  current  state  and  carry  no  information  based  on  the  next  time  step.  The  equations  pro¬ 
viding  these  predictions  are  called  projection  equations  and  are  as  follows 


Ci  =  Fk*k 

(17) 

=  FkPkFk  +  Qk . 

(18) 

Once  this  stage  is  completed,  recursion  continues  with  the  calculation  of 
the  Kalman  gain  for  the  next  time  step. 

C.  THE  NONLINEAR  CASE 

The  Kalman  filter  has  been  theoretically  proven  to  be  the  best  estimator  for  the 
linear  case  estimation  problem  described  in  the  previous  section  [1],  [2],  However,  a 
number  of  current  applications  include  nonlinearities  and  additive  non-Gaussian  noise 
which  makes  the  Kalman  filter  inadequate.  This  has  led  to  the  development  of  advanced 
algorithms  fitted  to  the  requirements  of  each  specific  case. 

1.  The  General  Nonlinear  Case 

The  general  form  of  the  nonlinear  case  model  can  be  specified  by  Eqs.  (3)  and  (4) 
under  the  assumption  that  either  or  both  functions/ and  g  are  nonlinear.  It  is  obvious  that 
Kalman  filter  cannot  be  directly  applied  in  this  kind  of  model.  Several  techniques  have 
been  developed  to  overcome  the  difficulties  introduced  by  the  nonlinearities,  considering 
the  specific  task  to  be  accomplished.  An  overview  of  the  most  commonly  used  ones  is  the 
motivation  for  the  following  sections. 

2.  The  Extended  Kalman  Filter  (EKF) 

The  EKF  is  used  in  various  real-time  applications  due  to  the  fact  that  it  is  less 
computationally  intensive  when  compared  to  other  nonlinear  estimation  techniques.  It  is  a 
regular  Kalman  filter  that  uses  local  linearization  based  on  a  first-order  Taylor  series  ex¬ 
pansion  about  the  current  estimate  of  the  state  and  its  covariance  matrix.  Let  the  model  of 
interest  be  the  one  specified  by  Eqs.  (1)  and  (2)  and  both  noise  sequences^  and  vk  have 
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the  same  properties  as  described  in  the  linear  case.  The  difference  Ax(t)  between  the  ac¬ 
tual  and  the  estimated  value  can  be  introduced  and  used  in  the  model  as  described  by  the 
following  set  of  equations: 


x(t)  +  Ax(t)  =  f  [x(t)  +  Av(0]  +  w{t) 


(19) 


y(t)  =  g  [x(0  +  Ax(t)]  +  v(0- 


(20) 


Assuming  that  A x{t)  is  small,  an  approximation  using  a  Taylor-series  expansion 
along  with  the  fact  that  x(t)  =  /[x(t)]  yields: 


x(t)  +  A x(t)  =  /  [x(t)]  +  —  A x(t)  +  w(t )  =>  A x(t)  =  rf-  A x(t)  +  w(t ) 

ox  ox 


y(t )  -  g  [*(0]  =  vf- Ax(0 + v(0- 

ox 


(21) 

(22) 


The  quantity  A x(t)  can  be  treated  as  the  state,  and  the  quantity  y(t  )  -  g  [x(/)]  can  be 

treated  as  the  measurement,  to  provide  a  linear  model  where  the  Kalman  filter  can  be  di¬ 
rectly  applied.  The  discrete  time  equivalent  system  uses  the  model  described  by  Eqs.  (3) 
and  (4)  where 


df[xk\ 


dx. 


xk 


Hk 


dg[xk] 

dxk 


(23) 


(24) 


The  procedure  to  be  followed  is  the  same  as  the  one  described  in  the  linear  case. 
A  block  diagram  indicating  the  calculations  that  are  to  be  executed  step  by  step  is  shown 
in  Fig.  2. 

The  EKF  described  in  this  section  uses  a  first-order  linearization  since  only  the 
first  term  of  the  Taylor-series  expansion  is  used.  More  accurate  results  may  be  obtained 
using  a  higher-order  EKF  that  retains  higher-order  tenns  of  the  Taylor  series,  but  the 
computational  intensity  is  significantly  increased. 
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Figure  2.  Block  Diagram  of  the  EKF 


2.  The  Grid-Based  Methods 

In  order  to  proceed  to  the  analysis  of  the  grid-based  methods,  first  a  statistical  ap¬ 
proach  of  the  estimation  problem  is  presented.  The  goal  remains  to  provide  a  filtered  es¬ 
timate  of  the  state  xk  at  time  step  k  given  the  available  set  of  measurements 

yk .  =  {yt,i  =  1, k }  up  to  time  step  k.  From  a  Bayesian  perspective,  the  problem  is  to  re¬ 
cursively  determine  a  measure  of  belief  in  the  state  xk  at  time  step  k,  taking  different  val¬ 
ues  and  utilizing  the  knowledge  of  the  available  set  of  measurements  y  k  [2],  Hence,  the 
posterior  probability  density  function  (pdf)  p(xk  \  yk )  is  constructed  and  updated  in  each 
time  step.  The  whole  procedure  can  be  described  as  follows  [2],  [3]. 


10 


The  initial  density  of  the  state  vector,  p(x0 )  ,  is  assumed  to  be  known  and  inde¬ 
pendent  of  any  measurement.  Estimation  of  the  pdf  in  each  time  step  is  obtained  recur¬ 
sively,  implementing  sequentially  the  prediction  and  update  stage.  Assuming  that  the  pdf 
P  (xk-\  I  y*_i )  in  bmc  step  k  - 1  is  available,  the  estimation  of  the  pdf  at  the  next  time  step 
without  knowledge  of  the  related  measurement  (that  is  the  prediction)  is  given  by 

p  (xk  I  y*.i )  =  \p  (**  I  **-i  )p  ixk- 1 1  y*.i )  dxk~ i  (25) 

where  the  pdf  p(xk  \  xk_x )  is  fully  defined  based  on  the  state  Equation  (3)  and  the  statis¬ 
tics  of  the  process  noise  sequence  wk .  The  update  stage  evolves  the  prediction  by  utiliza¬ 
tion  of  the  next  measurement  yk  and  updating  by  using: 


p{xk\yk)  =  p{xk\yk^k-x) 


fUKlfMr i) 
/>(y*ly*-i) 


(26) 


where  the  nonnalizing  pdf  p[yk  y,.., )  is  fully  defined  on  the  basis  of  the  measurement 
Equation  (4)  and  the  statistics  of  the  measurement  noise  vk  . 

For  the  grid-based  methods  the  discretization  of  the  above  formulation  is  used. 
Assuming  that  the  state  space  consists  of  N  discrete  states  at  time  step  k  - 1  denoted 
by  x)_, ,  i  =  1 ,...,  A  and  the  conditional  probability  of  that  state  at  the  same  time  step  is 

tenned  as  aik_X\k_x ,  the  posterior  pdf  can  be  written  as: 

N 

p{xk\yk)=p  ixk~  1 1  y*.i )  =  X  °i-i\k-i<5(xk-i  -  4-i )  (27) 

i= 1 

Substitution  of  Eq.  (27)  into  Eqs.  (25)  and  (26)  provides  the  prediction  and  update  equa¬ 
tions  for  the  grid-based  methods: 

N 

p(xk  yk-i )  =  X  *4-i 5{xk  ~  4 )  (28) 

i=i 

N 

where  0)‘kk_{  =X^-n*-i/>(4  I XD  and 

j= i 
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(29) 


N 

p{xk\yk)  =  YjC0'k\ks(xk-xik) 

i= l 

/  N 

where  6J?k[k  =  (t*  <-)/Z 1 4 )  ■ 

The  recursive  propagation  of  the  pdf  that  is  described  by  Eqs.  (28)  and  (29)  pro¬ 
vides  enough  information  to  proceed  to  the  optimal  state  estimate,  depending  on  the  crite¬ 
rion  that  is  established  for  any  case.  That  is,  if  the  minimum  mean  square  error  (MMSE) 
is  the  criterion,  then  the  estimate  is  given  by  [2]: 

xk\kSE  =  e {xk  I y*} -\xkp{xk  I y k ) d%k ■  (30) 

In  the  case  of  the  maximum  a  posteriori  (MAP)  estimate,  the  maximum  value  of  the  pdf 
is  used  [2]: 

xkf  =  ar8  max  (-A  I  >'/.)•  (31) 

The  use  of  the  grid-based  methods  is  constrained  by  the  assumption  that  the  state 
space  has  to  be  discretized  in  a  finite  number  of  states  [2].  These  constraints,  along  with 
the  extremely  high  computational  cost  in  multi-dimensional  state  space  cases  and  the  re¬ 
quirement  for  sufficiently  dense  grid  [3],  significantly  restricts  the  number  of  applications 
to  which  this  technique  can  be  applied. 

3.  The  Particle  Filter 

The  concept  of  particle  filtering  was  originally  proposed  back  in  1954  [2]  in  the 
form  of  the  Sequential  Monte  Carlo  (SMC)  estimation  based  on  particle  representation  of 
probability  densities.  However,  despite  the  continuous  theoretical  exploration  of  the  ini¬ 
tial  idea  during  the  1960s  and  1970s,  no  effort  to  apply  it  to  a  specific  application  took 
place.  The  main  constraint  obviously  was  the  amount  of  computational  effort  that  is  re¬ 
quired  to  apply  such  a  technique.  The  fact  that  the  computational  power  of  the  machines 
at  that  time  was  quite  poor  leads  to  the  conclusion  that  the  implementation  of  a  complete 
system  using  the  proposed  technique  was  not  feasible.  Since  computational  power  has 
been  tremendously  increased  during  the  past  decades,  the  above  mentioned  constraint  has 
been  eliminated.  Particle  filter  applications  in  various  fields  such  as  wireless  communica¬ 
tions  [4],  target  tracking  [5],  and  navigation  systems  [6]  have  recently  been  proposed. 
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a.  Overview  of  the  Particle  Filter  (PF) 

The  idea  of  the  PF  technique  is  based  on  the  statistical  approach  of  the 
nonlinear  estimation  problem  discussed  previously.  The  main  feature  is  the  representation 
of  the  density  function  of  interest  using  a  random  set  of  samples  for  the  state  value  with 
associated  weights.  This  kind  of  representation  recursively  leads  to  the  estimation  of  the 
pdf  for  the  next  time  step,  taking  into  account  the  measurement  associated  with  it.  If  the 
number  of  samples  used  in  each  iteration  is  sufficient,  the  representation  of  the  pdf  is 
quite  accurate  and  it  provides  good  estimates. 

Let  \k  represent  the  set  of  the  target  values  of  the  state  for  time  step  0  to 

k.  Assuming  that  in  each  time  step  a  number  of  N samples  are  drawn,  the  set  |x^, CQky  ^ 

of  the  support  points  with  the  associated  weights  fully  characterizes  the  posterior  pdf 
p  (xA  \yk).  The  weights  are  selected  in  such  a  way  that  the  weighting  vector  is  normal- 

N 

ized,  that  is  0)‘k  =1  for  each  time  step  k.  Under  these  assumptions,  Eq.  (29)  may  be 

/= i 

used  in  order  to  provide  an  approximation  of  the  posterior  fdtered  density  p  (xk  y  t ) . 

b.  The  Sequential  Importance  Sampling  (SIS)  Algorithm 

The  basis  of  each  PF  technique  is  the  SIS  algorithm  (or  otherwise  called 
Bootstrap  Filtering  [7]).  According  to  this  algorithm  the  particles,  meaning  the  state 
samples,  are  propagated  in  each  iteration  by  utilization  of  two  stages,  the  prediction  and 
the  update. 

During  the  prediction  stage,  N  samples  of  the  process  noise  wk  are  drawn 

according  to  the  statistics  of  the  noise  sequence.  Then,  each  sample  point  of  the  state  is 
propagated  using  the  state  equation,  without  taking  into  account  the  corresponding  impor¬ 
tance  weights.  This  provides  a  posterior  prediction  of  the  particles  for  the  next  time  step 
according  to  the  equation 

xL=fk(4M)  (32) 

where  i  =  1  represents  the  index  number  of  the  sample  and  fk  is  the  function  re¬ 
lated  to  the  state  at  time  step  k  as  defined  in  Eq.  (3). 
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During  the  update  stage  the  important  weights  are  updated  using  the  in¬ 
formation  provided  by  the  measurement  sequence  up  to  time  step  k.  The  related  equation 
is 

®,k+x=®lp(yk+i\x[+x)  (33) 


where  the  likelihood  function  p(yk+l  \  x‘k+l )  depends  on  the  measurement  equation  intro¬ 
duced  by  Eq.  (4)  and  the  statistics  of  the  measurement  noise  vk .  Before  finalizing  this 

stage,  the  weighting  vector  has  to  be  normalized  and  a  new  value  is  applied  to  each 
weight  using  the  equation 


i= 1 


(34) 


where  the  superscript  o  indicates  the  weight  value  originally  computed  using  Eq.  (33)  and 

CO'k+l  is  the  weight  value  of  the  i-th  particle  to  be  propagated  to  the  next  iteration. 

c.  The  Resampling  Procedure  -  Sampling  Importance  Resampling 
(SIR)  Algorithm 

EInfortunately,  in  practice,  the  SIS  algorithm  has  a  significant  drawback. 
After  a  certain  number  of  steps,  only  one  particle  with  significant  associated  weight  value 
is  left  [7].  The  rest  of  the  weights  are  negligible  leading  to  two  unwanted  effects,  low  ac¬ 
curacy  and  a  large  computational  effort  to  propagate  particles  of  negligible  significance 
regarding  their  contribution  to  the  approximation  of  the  desired  pdf  p(xk  y ) .  This  fact 

is  known  as  the  degeneracy  problem  [2]  and  its  existence  implies  the  need  for  an  addi¬ 
tional  stage,  resulting  in  a  more  robust  algorithm  than  the  SIS  one. 

The  key  point  to  the  solution  of  the  degeneracy  problem  is  the  definition 
of  a  new  parameter  called  the  effective  number  of  samples,  which  is  estimated  by  the  re¬ 
lationship  [3]: 


1 

~N  T 

IN) 


(35) 
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where  cdk  are  the  normalized  values  of  the  weights  obtained  at  the  end  of  the  update  stage 

using  Eq.  (34).  Whenever  Neff  has  a  small  value,  the  degeneracy  phenomenon  is  severe 

and  a  new  stage  called  resampling  has  to  be  executed.  During  this  stage,  particles  with 
negligible  importance  weights  are  discarded  and  replaced  by  samples  with  large  weights. 
The  importance  weight  of  each  remaining  particle  is  a  measure  of  its  probability  of  repe¬ 
tition.  Excluding  the  extreme  cases  of  a  uniform  distribution  of  the  importance  weights 

( Neff  =  N  )  and  of  N  - 1  zero  weights  ( Neff  =  1 ),  the  resampling  procedure  provides  an 
effective  scheme  in  avoiding  degeneracy. 

The  last  parameter  that  has  to  be  determined  in  order  to  apply  resampling 
is  the  decision  threshold  for  the  effective  number  of  samples.  There  is  no  general  guide¬ 
line  for  its  selection.  However,  it  has  to  be  taken  into  account  that  a  small  threshold  leads 
to  high  resampling  ratio  implying  high  computational  cost,  while  a  large  value  of  the  de¬ 
cision  threshold  leads  to  non-effective  resampling  because  in  each  iteration  some  parti¬ 
cles  with  negligible  weights  are  reproduced. 

The  algorithm,  including  the  resampling  stage  where  necessary,  is  called 
Sampling  Importance  Resampling  (SIR).  In  this  thesis,  a  PF  based  on  this  algorithm  is 
developed  and  evaluated. 

D.  ATTITUDE  REPRESENTATION 

In  this  section  a  short  description  of  the  available  attitude  representation  methods 
along  with  the  basic  of  rigid  body  kinematics  equations  are  presented.  The  quaternion 
representation  and  the  related  kinematics  equations  are  emphasized  due  to  their  extensive 
use  in  the  modeling  and  simulation  of  the  system  described  in  Chapter  IV. 

The  attitude  representation  of  a  rigid  body  can  be  described  in  terms  of  rotating 
the  orientation  of  the  body  coordinate  system  to  align  with  another  one.  The  following 
methods  can  be  applied  to  describe  these  kinds  of  rotations. 

1,  The  Euler  Angles 

Euler  angles  define  a  series  of  three  successive  rotations  around  the  body  coordi¬ 
nate  system  in  order  to  achieve  the  desired  orientation  [8].  Each  rotation  occurs  around 
the  original  body  axes.  Since  there  are  twelve  possible  ways  to  arrange  sequentially  the 
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three  axes,  there  are  twelve  different  possible  set  of  Euler  angles.  In  this  thesis  the  orien¬ 
tation  roll  <p  (rotation  about  the  x  axis),  pitch  #  (rotation  about  the  y  axis)  and  yaw  i//  (rota¬ 
tion  about  the  z  axis)  is  used.  In  this  case  a  rotation  can  be  represented  by  the  following 
set  of  rotation  matrices: 


C\((p)  = 


C2(0)  = 


C3(W)  = 


cos  <p 

sin^?  0 

-sin  cp 

cos^  0 

0 

0  1 

cos# 

0  -  sin  # 

0 

1  0 

sin# 

0  cos# 

1  0  0 
0  cos  y/  sin  y/ 
0  -sin^  cos y/ 


(36) 


The  product  of  these  three  matrices  is  the  rotation  matrix  between  the  initial  and 
final  orientations  of  the  body  and  is  a  function  of  all  three  Euler  angles. 

2.  Quaternion  Representation 

The  quaternion  q  is  a  four-element  vector  which  fully  describes  a  rotation  in  a 
three  dimensional  space.  The  first  element  of  the  quaternion  is  a  scalar  value  while  the 
remaining  three  elements  are  vector  components.  The  scalar  value  represents  a  measure 
of  the  magnitude  of  rotation.  The  remaining  three  elements  are  proportional  to  the  Euler 
axis  e  around  which  the  rotation  occurs  [8].  Letting  6e  be  the  angle  of  rotation,  the  qua¬ 
ternion  components  are  defined  as  follows  [9] : 

0e 

=  cosy 

.  oe 

<h  =^smy 

(37) 

•  Oe 

q2=eysm  — 

■  oe 

q,=e  sin  — 

2 
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where  ex ,  ev  and  e,  are  the  three  normalized  to  unitary  magnitude  elements  of  the  Euler 
axis  with  respect  to  x,  y  and  z  axes,  respectively. 

A  very  important  property  of  the  quaternions  is  the  normalization  constraint 
which  is  determined  by 

ql  +  q{  +  +  q]  =  1  •  (38) 

The  above  equation  means  that  the  four  elements  of  the  quaternion  vector  are  mutually 
dependent.  This  makes  sense  since  a  four-dimensional  vector  is  used  to  describe  a  rota¬ 
tion  in  a  three-dimensional  space.  The  reason  that  the  quaternion  representation  is  exten¬ 
sively  used  in  attitude  representation  is  that  it  does  not  introduce  singularities  [10]  and 
requires  less  computational  effort  to  propagate  when  compared  to  representations  includ¬ 
ing  rotation  matrices.  However,  the  mapping  between  quaternions  and  Euler  angles  is  not 
unique  [11],  [12]  since  any  pair  of  quaternions  qand  -q  represent  exactly  the  same  rota¬ 
tion.  On  the  other  hand,  the  Gibbs  vector  provides  a  three-dimensional  one  to  one  repre¬ 
sentation  of  a  rotation  vector. 

3.  The  Gibbs  Vector 

The  Gibbs  vector  g  is  a  representation  directly  related  to  the  quaternions  by  the 
following  relationship: 

q\ 

1  a„ 

g  =  -  q2  =  ^-.  (39) 

q0  2 

LfcJ 

In  order  to  understand  the  relation  between  the  quaternion  and  the  Gibbs  vector,  recall 
that  the  quaternion  vector  is  defined  on  a  four-dimensional  sphere  [11].  The  Gibbs  vector 
can  be  considered  as  a  gnomonic  projection  of  the  quaternion  vector  from  the  sphere  to 
the  vector  g  as  shown  in  Fig.  3.  The  use  of  the  Gibbs  vector  eliminates  the  2:1  mapping 
issue  of  the  quaternion  representation.  However,  it  is  clear  that  it  generates  a  singularity 
for  180°  rotations  which  limits  its  use  in  representations  of  rotations  in  the  interval 
(-71, 71)  .  Hence,  the  use  of  the  Gibbs  vector  is  not  adequate  when  global  representation  of 
rotations  is  desired. 


17 


Figure  3.  Gibbs  vector  as  a  gnomonic  projection  (After  Ref.  [1 1].) 


4.  Attitude  Kinematics 

In  order  to  propagate  the  quaternion  vector,  an  introduction  to  the  quaternion 
kinematics  equation  is  necessary.  This  equation  describes  the  evolution  of  the  quaternion 
vector  in  time  with  respect  to  the  angular  rate  given  by  the  following  relationship: 


q  =  ^(o>)q 


(40) 


where  o>  = 


ay 

CO. 


is  the  angular  rate  vector  of  the  body  and  the  matrix  £1  is  of  4x4  di¬ 
mension  and  it  can  be  calculated  by  the  equation: 

fl(w) 


-[tox]  0) 
-tor  0 


(41) 


where  [to  x]  denotes  the  cross  product  matrix  of  the  angular  rate  vector,  which  in  the 
body  coordinate  system  is  given  by: 


[tax]  = 


0  -co.  coy 

co,  0  -cox 

-co,  co,  0 


(42) 
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By  discretizing  Eq.  (40)  with  small  time  increments  and  utilizing  Eqs.  (41)  and 
(42),  a  discrete -time  dynamic  model  for  the  quaternion  propagation  can  be  derived. 

In  this  chapter,  various  solutions  to  the  estimation  problem  were  discussed.  The 
use  of  PF  is  of  particular  interest.  In  order  to  apply  particle  filtering,  a  dynamic  model 
based  on  the  attitude  kinematics  that  were  previously  presented  has  to  be  developed.  The 
derivation  of  such  a  model  for  the  NPS  TASS  is  the  main  point  to  be  addressed  in  the 
next  chapter. 
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III.  DYNAMIC  MODELING  FOR  THE  NPS  TASS 


The  objective  of  this  chapter  is  the  determination  of  a  dynamic  model  for  the  NPS 
TASS.  In  particular  this  model  is  based  on  the  quaternion  representation  and  it  is  used 
both  for  simulation  of  the  platform  and  the  estimation  by  particle  filtering.  At  first,  an 
overview  of  the  TASS  is  presented,  then  the  rate  gyroscope  along  with  the  associated  dis¬ 
turbances  is  discussed.  Finally,  the  dynamic  model  based  on  the  attitude  kinematics  pre¬ 
sented  in  the  previous  chapter,  is  developed. 

A.  SYSTEM  OVERVIEW 

The  NPS  TASS  is  a  platform  which  can  simulate  the  three-axis  motion  of  a 
spacecraft.  It  is  supported  on  a  spherical  air  bearing  which  provides  a  frictionless  motion 
in  order  to  emulate  a  space  environment.  Basically,  a  thin  film  of  air  is  trapped  between 
the  lower  spherical  end  of  the  simulator  and  a  semi-spherical  cup  of  matched  size  which 
is  mounted  on  the  ground.  A  detailed  description  of  the  system  is  found  in  Refs.  [13]  and 
[14]. 

Various  control  components  and  sensors  are  installed  in  the  TASS  in  order  to 
provide  enough  data  for  simulation  purposes.  Three  variable-speed  control  moment  gyro¬ 
scopes,  rate  gyros,  two-axis  analog  sun  sensors,  an  IR  sensor  and  two  inclinometers  are 
included.  In  this  thesis  the  rate  gyros  are  of  particular  interest  and  they  are  described  in 
tenns  of  their  dynamics  and  disturbances. 

B.  RATE  GYROSCOPES 

Rate  gyroscopes  are  generally  used  to  provide  angular  rate  data  to  the  spacecraft 
control  system.  Different  types  of  rate  gyroscopes,  such  as  mechanical  and  laser,  have 
been  developed.  Regardless  of  the  type  of  the  gyroscope,  the  task  to  be  accomplished  is 
to  sense  angular  rate  measurements  of  the  satellite  in  all  three  dimensions  and  with  re¬ 
spect  to  the  satellite  body  coordinates. 

There  are  two  different  types  of  errors  included  in  a  set  of  noisy  measurements 
provided  by  a  rate  gyroscope,  the  bias  and  the  noise.  The  main  reasons  for  the  presence  of 
these  errors  are  the  manufacturing  imperfections,  misalignment  and  the  degradation  of 
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the  gyroscope  through  time.  The  measured  value  of  the  angular  rate  vector  provided  by 
the  rate  gyroscopes  can  be  modeled  using  the  equation: 

©m(0  =  G>,(0  +  b(0  +  n(f)  (43) 

where  vector  notation  is  used  to  indicate  a  three-dimensional  quantity,  com  indicates  the 
measured  value,  to,  indicates  the  true  value,  b(7)  is  the  bias  vector  and  n (/)  is  the  three- 

dimensional  noise  vector  consisting  of  independent  white-noise  sequences. 

C.  DERIVATION  OF  THE  DYNAMIC  MODEL 

The  availability  of  a  set  of  measurements  of  the  angular  rate  vector  allows  the 
generation  of  a  part  of  the  state  equation  using  Eqs.  (40)  and  (43).  From  them,  an  expres¬ 
sion  of  the  quaternion  vector  in  terms  of  the  measured  values  of  the  angular  rate  can  be 
generated: 

q(0  =  7,  ^  K„  (0  -  b(0  -  n(0]  q(0  (44) 

where  Q,  is  the  matrix  presented  in  Eq.  (41). 

The  presence  of  a  slowly  varying  bias  can  be  modeled  as  a  random  walk  stochas¬ 
tic  process.  This  results  in  the  differential  equation: 

b(0  =  w  (t)  (45) 

where  w (t)  is  a  three-dimensional  noise  vector  consisting  of  independent  white  noise  se¬ 
quences  and  is  uncorrelated  to  the  noise  vector  n(t)  referred  above.  This  statistical  prop¬ 
erty  of  the  bias  forces  its  inclusion  as  part  of  the  state  vector  in  the  dynamic  model. 

As  far  as  the  measurement  equation  of  the  model  is  concerned,  the  combined 
measurements  of  the  two  inclinometers  and  the  IR  sensor  can  be  utilized  to  provide  a  set 
of  measured  Euler  angles.  Since  the  quaternion  is  part  of  the  state,  the  following  set  of 
equations  relating  the  quaternion  vector  to  the  Euler  angles  can  be  used: 
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arctan 


E  = 


<P 

0 

¥ 


'2q0q2  -2gxq^ 
,  1  -  2ql  -  2q\  J 
arcsin  (2qlq2  +  2  q0q3 ) 


arctan 


'  2q0qx-2q2qls 

v  1  —  2q1  —2qJ  y 


(46) 


where  E  denotes  the  set  of  Euler  angles  using  the  orientation  presented  in  the  previous 
chapter  and  q; ,  i  =  0, 1, 2, 3  are  the  elements  of  the  quaternion  vector  introduced  by  Eq. 

(37).  However,  since  the  data  are  provided  by  imperfect  sensors,  the  modeling  should  in¬ 
clude  the  noise  term.  Hence,  the  measurement  equation  can  be  written  in  the  form: 

Em(0  =  E(0  +  v(0  (47) 


where  Em  ( t )  is  the  available  set  of  measured  Euler  angles  and  v(t)  is  a  three-dimensional 
noise  vector  consisting  of  independent  white-noise  sequences. 


Let  the  nonlinear  function  presented  by  Eq.  (46)  be  denoted  by  h,  that  is 
E(t)  =  h  [q(0]  •  Encapsulating  all  the  above,  the  state  space  model  can  be  fully  deter¬ 
mined  by  the  following  set  of  equations: 


q(0 

>(oJ 

^K„(0-b(0-n(0]q(0 

w(0 


(48) 


E»(0  =  Mq(0]+v(0- 


(49) 


The  final  step  is  the  discretization  of  the  existing  model.  Assuming  that  the  sam¬ 
pling  interval  is  equal  to  At ,  Eqs.  (48)  and  (49)  lead  to  the  following  discrete-time  state 
space  model: 


q  \k + i] 

q  [k] 

1 

b  [A  + 1] 

b[A']_ 

l 

7T O [<*>,„  [k]  ~ b [k]  - n [k]j q [k] 


w 


[*] 


At 


(50) 
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(51) 


E„,  [k]  =  h(q[k])  +  \[k]. 

The  accuracy  of  the  model  depends  on  the  variances  of  the  noise  sequences 
n  [k] ,  w  [k]  and  v  [k] ,  as  well  as  the  choice  of  the  sampling  interval  At .  As  indicated  by 

Eq.  (50)  the  value  of  the  matrix  Q  is  considered  to  be  constant  between  two  successive 
iterations.  This  is  an  approximation  resulting  from  the  discretization  of  the  model.  The 
selection  of  a  small  value  of  the  sampling  interval  is  required  in  order  to  avoid  the  build¬ 
up  of  large-scale  errors  in  the  estimation  process. 

D.  SIMPLIFIED  DYNAMIC  MODEL  FOR  THE  UNBIASED  ONE-AXIS 

ROTATION  CASE 

The  model  described  above  contains  a  7-dimensional  state  vector  which  makes 
the  mathematical  modeling  quite  complex.  However,  when  estimating  an  unbiased  one- 
axis  rotation  the  representation  provided  by  Eqs.  (50)  and  (5 1)  can  be  significantly  re¬ 
duced.  The  assumptions  for  the  existence  of  noise  sequences  in  both  angular  rate  and  roll 
angle  measurements  still  hold.  These  are  the  external  disturbances  which  affect  the  atti¬ 
tude  detennination  in  an  undesirable  way  and  their  impact  is  to  be  eliminated  using  the 
particle  filter  implementation. 

In  the  one-axis  rotation  case  described  in  this  section,  let  the  rotation  be  around 
the  x-axis,  so  both  pitch  and  yaw  angles  are  equal  to  zero,  at  all  times.  Hence,  the  second 
and  third  elements  of  the  Euler  angle  vector  can  be  ignored.  The  same  consideration  can 
be  applied  to  the  angular  rate  vector,  with  only  the  x-axis  component  not  being  zero.  As  a 
consequence,  the  q2  and  q2  elements  of  the  quaternion  vector  can  be  ignored  in  Eq.  (37) 
since  no  rotation  around  the  y  or  z  axis  occurs.  This  leaves  only  two  of  the  quaternion 
elements  with  a  nonzero  value,  q0  and  qx .  Applying  the  normalization  constraint  intro¬ 
duced  in  Eq.  (38),  an  implementation  using  a  scalar  state  can  be  accomplished. 

The  model  equations  are  derived  using  ql  as  the  state  of  interest.  The  normaliza¬ 
tion  constraint  becomes: 

ql  +  ql  =  1  =>  q{  =  ±^~q20.  (52) 
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The  4-dimensional  matrix  Q  can  be  expressed  in  tenns  of  the  measured  angular 
rate  around  the  x-axis  C0m  using  the  Eq.  (41): 

0  0  0  (Om 

0  0  (Om  0 

0  -com  0  0 

-a  0  0  0 

m 

Then,  the  attitude  kinematics  Equation  (40)  can  be  reduced  to: 


<7°i  ir-^?1 

Ji\  2  |_  O)mq0 


(54) 


In  this  chapter,  the  target  system  (i.e.,  the  NPS  TASS)  was  presented  and  the  es¬ 
timation  problem  was  established.  The  dynamic  model  was  derived  and  simplified  for  the 
scalar  case  of  the  one-axis  rotation.  The  development  of  the  PF  and  its  evaluation  in  pro- 
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viding  an  adequate  solution  to  the  compensation  of  the  additive  noise  is  the  main  topic  of 
interest  discussed  in  the  following  chapter. 
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IV.  PERFORMANCE  ANALYSIS 


This  chapter  presents  the  simulation  results  for  evaluating  the  particle  filter  esti¬ 
mator  in  various  cases.  Firstly,  KF  and  PF  are  applied  to  a  linear  model  in  a  Gaussian  en¬ 
vironment  and  the  estimates  are  compared  in  terms  of  the  mean  square  error  (MSE).  Sec¬ 
ondly,  the  same  model  in  additive  biased  non-Gaussian  noise  is  tested  using  various  PF 
implementations.  Lastly,  a  PF  estimator  for  the  attitude  detennination  of  the  spacecraft 
platform  discussed  in  the  previous  chapter  is  presented.  Throughout  the  chapter,  the  im¬ 
portance  of  the  right  choice  of  the  number  of  particles  for  the  PF  implementation  is  dis¬ 
cussed. 

A.  PF  EVALUATION  FOR  THE  LINEAR  MODEL 

It  is  well  known  that  KF  is  the  optimal  estimator  for  linear  Gaussian  models  [2]. 
The  reason  is  the  fact  that  all  a  priori  and  a  posteriori  probability  density  functions  are 
Gaussian  and,  therefore,  fully  defined  by  their  mean  and  covariance. 

It  is  reasonable  to  expect  that  a  PF  with  a  large  number  of  particles  provides  re¬ 
sults  comparable  to  the  KF  for  a  linear  Gaussian  model.  In  this  section  the  tradeoff  be¬ 
tween  the  number  of  particles  (and  therefore  the  computational  cost)  and  the  PF  perform¬ 
ance  is  evaluated  for  various  examples. 

The  linear  model  that  is  used  in  this  simulation  section  is  the  one  described  by  the 
following  set  of  equations: 

x[k  +  l]  =  0.9x[k]  +  w[k]  (59) 

y\k\  =  x[k]  +  v[k]  (60) 

where  x\k\  is  the  state  to  be  estimated,  v[£]  is  the  available  measurement,  vv [ /c  ]  and 

v\k\  are  white  noise  sequences  with  unit  variance.  The  convenience  that  this  particular 

measurement  equation  provides  is  the  direct  association  of  the  state  to  the  measurement, 
so  the  measurement  error  can  be  directly  compared  to  the  state  error  provided  by  both 
filters.  The  initial  state  value  was  assumed  to  be  equal  to  zero  in  all  simulations. 
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A  simulation  sample  result  regarding  the  performance  of  the  Kalman  filter  for  this 
model  is  presented  in  Fig.  4.  In  this  simulation  a  sequence  of  100  points  was  created 
based  on  Eq.  (59).  Then  the  Eq.  (60)  was  used  to  provide  an  available  set  of  measure¬ 
ments.  The  Kalman  filter  algorithm  was  implemented  and  the  filter  output  is  plotted 
along  with  the  absolute  error  of  the  estimation.  It  is  clear  that  the  filtered  output  provides 
much  better  estimation  of  the  state  than  the  available  set  of  measurements.  A  measure  to 
evaluate  the  performance  of  the  Kalman  filter  is  the  mean  square  error  (MSE).  The  MSE 
of  the  estimation  was  computed  to  be  0.44857  while  the  MSE  of  the  measurement  was 
0.98838.  This  means  that  in  this  case  the  use  of  KF  decreases  the  MSE  of  the  estimation 
by  54.6%. 


Plot  of  xk,  estimation  of  xk,  measurement  yk  and  absolute  error  vs  sample  number 


Figure  4.  Demonstration  of  KF  performance 


In  order  to  compare  the  KF  with  the  PF  estimates,  numerous  simulations  were 
executed  for  different  number  of  particles  for  the  PF.  In  Figs.  5  through  10  the  results  of 
1000  simulations  per  case  are  presented  in  the  form  of  histograms.  In  particular,  the  re- 
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suits  testing  the  PF  for  10,  20,  100,  500,  2000  and  5000  particles  are  analyzed.  For  each 
simulation,  a  sequence  of  50  points  (i.e.,  time  steps),  based  on  the  model  described  by 
Eqs.  (59)  and  (60),  was  generated  and  estimated  using  both  Kalman  and  particle  fdters. 
The  measure  of  comparison  between  the  Kalman  filter  and  the  PF  is  the  MSE. 


MSE  distribution  for  KF  and  PF  using  10  particles 


Figure  5.  MSE  comparison  for  KF  and  PF  using  10  particles 


In  the  first  case,  10  particles  were  used  for  the  PF  implementation.  For  each  simu¬ 
lation  the  MSE  of  the  measurement,  the  KF  and  the  PF  were  computed  and  plotted  as 
single  points.  The  results  are  shown  in  Fig.  5.  It  is  clear  that  in  this  case  the  KF  outper¬ 
forms  the  PF.  The  percentage  of  better  KF  performance  was  computed  to  be  99.2%  of  the 
tested  runs.  Finally,  the  average  MSEs  of  all  simulations  were  computed  to  be  0.99848 
for  the  measurement,  0.59178  for  the  KF  estimate  and  0.80476  for  the  PF. 

The  implication  of  these  results  is  that  even  if  the  PF  reduces  the  measurement  er¬ 
ror,  it  is  unlikely  to  provide  a  good  estimate  for  the  state.  This  is  due  to  the  insufficient 
number  of  particles  used  in  this  simulation.  The  next  simulation  where  20  particles  are 
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used  indicates  the  improvement  of  the  PF  performance  when  more  particles  are  used. 
These  results  are  presented  in  Fig.  6. 


MSE  distribution  for  KF  and  PF  using  20  particles 


Figure  6.  MSE  comparison  for  KF  and  PF  using  20  particles 

Comparing  Figs.  5  and  6,  the  improvement  of  the  PF  is  obvious.  The  KF  still  out¬ 
performs  PF  by  a  significant  percentage  of  94.6%.  This  implies  a  still  poor  performance 
for  the  PF.  The  average  MSEs  of  all  simulations  were  computed  to  be  0.98340  for  the 
measurement,  0.58888  for  the  KF  estimate  and  0.68623  for  the  PF.  Obviously,  there  is 
more  improvement  to  be  accomplished  by  selecting  more  particles  to  be  propagated  by 
the  PF. 

The  next  set  of  simulations  was  executed  by  choosing  100  particles  for  the  pa¬ 
rameter  of  interest.  The  resulting  plot  is  shown  in  Fig.  7.  The  increase  of  the  number  of 
particles  to  this  value  has  resulted  in  a  significant  improvement  of  the  PF  performance. 
The  average  MSEs  of  all  simulations  were  computed  to  be  0.99105  for  the  measurement, 
0.59027  for  the  KF  estimate  and  0.61723  for  the  PF  one.  The  improvement  of  the  PF  per- 
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formance  can  be  easily  visualized  by  the  reduction  of  the  distance  between  the  plotted  KF 
and  PF  MSEs  that  can  be  observed  in  Fig.  7  when  compared  to  Figs.  5  and  6. 


MSE  distribution  for  KF  and  PF  using  100  particles 


MSE 


Figure  7.  MSE  comparison  for  KF  and  PF  using  100  particles 


Figure  8  presents  the  results  using  500  particles.  The  PF  performance  is  once 
more  improved.  The  average  MSEs  of  all  simulations  are  0.97850  for  the  measurement, 
0.58419  for  the  KF  estimate  and  0.58812  for  the  PF  one.  The  percentage  of  better  KF 
performance  is  reduced  to  62.9%.  Recall  that  the  PF  converges  to  the  KF  one  in  the  theo¬ 
retical  case  of  infinite  number  of  particles.  This  means  that  from  a  statistical  point  of 
view  the  lower  bound  for  better  KF  performance  is  50%.  Evaluating  the  PF,  the  conclu¬ 
sion  is  that  500  can  establish  a  lower  bound  for  the  number  of  particles  in  order  to  get  re¬ 
sults  of  high  accuracy  for  this  estimation  problem. 
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MSE  distribution  for  KF  and  PF  using  500  particles 
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Figure  8.  MSE  comparison  for  KF  and  PF  using  500  particles 


Concluding  the  research  for  the  linear  model  case,  two  more  sets  of  simulations 
using  2000  and  5000  particles  were  executed.  The  resulting  plots  are  shown  in  Figs.  9 
and  10,  respectively.  For  the  2000  particles  case,  the  average  MSEs  of  all  simulations  are 
0.98517  for  the  measurement,  0.58908  for  the  KF  estimate  and  0.58977  for  the  PF  one. 
The  percentage  of  better  KF  performance  is  computed  to  be  62.6%.  The  performance  of 
the  PF  is  evaluated  as  sufficient,  considering  that  the  average  distance  ratio  between  the 
MSEs  of  the  KF  and  the  PF  is  less  than  0.12%.  This  fact  can  be  easily  visualized  in  the 
related  figure  by  noticing  that  for  a  significant  number  of  simulations  the  green  bars  rep¬ 
resenting  the  PF  MSE  lay  partially  over  the  blue  bars  representing  the  KF  MSE. 
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MSE  distribution  for  KF  and  PF  using  2000  particles 


Figure  9.  MSE  comparison  for  KF  and  PF  using  2000  particles 

The  resulting  plot  for  the  5000  particles  case  is  shown  in  Fig.  10.  The  percentage 
of  better  KF  performance  is  66.1%  indicating  that  the  required  number  of  particles  for  the 
particular  application  has  exceeded  its  saturation  point.  The  resulting  average  MSEs  are 
0.98403  for  the  measurement,  0.58900  for  the  KF  estimate  and  0.58945  for  the  PF  one. 
The  performance  of  PF  can  be  evaluated  as  excellent.  However,  the  computational  effort 
required  to  provide  these  results  is  considerable.  The  average  distance  ratio  between  the 
MSEs  of  the  KF  and  the  PF  is  less  than  0.08%,  verifying  the  tradeoff  between  the  compu¬ 
tational  cost  and  the  provided  estimation  accuracy. 

For  further  reference,  all  simulation  results  are  also  presented  in  Appendix  A  in 
the  form  of  scatter  plots. 
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MSE  distribution  for  KF  and  PF  using  5000  particles 


Figure  10.  MSE  comparison  for  KF  and  PF  using  5000  particles 


The  results  of  all  simulation  sets  are  summarized  and  tabulated  in  Table  1.  The 
improvement  ratio  for  each  set  is  computed  with  respect  to  the  measurement  error,  which 
is  defined  as: 
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where  /  stands  for  the  improvement  ratio,  subscripts  KF  and  PF  represent  the  related  fil¬ 
ter  estimates,  subscript  meas  refers  to  the  measurement  and  i  represents  the  simulation 
index  number  for  each  set. 


Sim 

set  # 

Number  of 

particles  used 

Average 

MSEmeas 

Average 

msekf 

Average 

MSEpf 

I KF 

‘  PF 

1 

10 

0.99848 

0.59178 

0.80476 

1.68725 

1.24718 

2 

20 

0.98340 

0.58888 

0.68623 

1.66995 

1.43305 

3 

100 

0.99105 

0.59027 

0.61723 

1.67898 

1.60564 

4 

500 

0.97850 

0.58419 

0.58812 

1.67497 

1.66378 

5 

2000 

0.98517 

0.58908 

0.58977 

1.67239 

1.67043 

6 

5000 

0.98403 

0.58900 

0.58945 

1.67068 

1.66940 

Table  1.  MSE  comparison  chart  of  KF  and  PF  performance  for  the  linear  model  case 


To  complete  the  study  for  this  section,  several  conclusions  can  be  mentioned. 
First,  the  number  of  particles  to  be  propagated  through  the  PF  is  important  to  the  filter 
performance.  Second,  for  real-time  applications  the  computational  intensity  is  an  impor¬ 
tant  issue.  Hence,  the  selection  of  the  amount  of  particles  has  to  be  made  with  respect  to 
both  accuracy  and  complexity.  As  a  guideline,  for  the  particular  model  tested,  a  selection 
of  500  to  2000  particles  would  be  a  wise  choice. 

B.  PF  EVALUATION  IN  NON-GAUSSIAN  ENVIRONMENT 

The  purpose  of  the  simulations  of  this  section  is  to  demonstrate  the  robustness  of 
PF  in  any  kind  of  additive  noise.  For  this  purpose  the  model  described  by  Eqs.  (59)  and 
(60)  is  used  to  generate  the  state  and  measurement  sequences.  The  only  assumption  dif¬ 
ferent  than  the  ones  made  in  the  previous  section  is  that  the  process  noise  sequence  w[k] 
is  the  square  of  a  white  Gaussian  noise  of  unit  variance  and,  therefore,  non-Gaussian  with 
mean  value  equal  to  1  and  standard  deviation  equal  to  V2  . 

The  KF  is  not  expected  to  work  effectively  in  this  case,  so  it  was  not  included  in 

the  resulting  plots.  The  comparison  can  be  made  between  the  MSE  of  the  measurement 
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and  the  MSE  of  the  estimation,  since  the  state  and  the  measurement  are  directly  associ¬ 
ated  with  no  imposed  scaling  or  addition  factor  through  the  measurement  equation.  For 
each  simulation,  sequences  of  100  points  (i.e.,  time  steps)  were  generated.  Each  simula¬ 
tion  set  consists  of  1000  runs.  The  number  of  particles  is  the  parameter  of  interest  and  the 
PF  was  tested  for  10,  50,  100,  500  and  2000  propagating  particles.  The  results  are  pre¬ 
sented  in  Figs.  1 1  through  15. 

The  resulting  plot  for  the  10  particles  case  is  shown  in  Fig.  11.  The  impact  of  the 
non-Gaussian  environment  is  quite  obvious.  In  most  cases  the  PF  is  unable  to  provide 
better  estimation  than  the  measurement  itself.  This  is  due  to  the  increased  value  of  the 
process  noise  variance  plus  the  biased  conditions  for  the  state  equation.  The  bad  perform¬ 
ance  of  the  PF  can  be  outlined  by  the  average  MSEs  which  are  0.99083  for  the  measure¬ 
ment  and  1.4976  for  the  estimate.  An  increase  of  the  number  of  particles  is  needed  in  or¬ 
der  to  reach  filter  convergence. 


MSE  distribution  for  PF  using  10  particles,  non  Gaussian  additive  noise 


MSE 


Figure  1 1 .  MSEs  for  the  non-Gaussian  noise  case  using  10  particles 
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The  next  simulation  was  executed  using  50  particles.  It  is  not  expected  to  provide 
acceptable  performance,  but  the  results  can  be  used  for  comparison.  The  resulting  plot  is 
presented  in  Fig.  12. 


MSE  distribution  for  PF  using  50  particles,  non  Gaussian  additive  noise 
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Figure  12.  MSEs  for  the  non-Gaussian  noise  case  using  50  particles 


The  significant  improvement  of  the  PF  performance  is  obvious  when  comparing 
Figs.  1 1  and  12.  For  the  50-particle  case,  the  estimates  are  better  than  the  measurements 
themselves  in  most  cases,  indicating  filter  convergence.  The  average  MSEs  were  com¬ 
puted  to  be  0.99605  for  the  measurement  and  0.75424  for  the  PF  estimate. 

From  the  results  presented  to  this  point,  the  conclusion  made  in  the  linear  case  on 
the  selection  of  the  number  of  particles  would  be  expected  to  hold  in  this  case  as  well. 
Hence,  the  simulations  for  500  and  2000  particles  are  of  significant  importance.  For 
comparison  reasons,  the  simulation  for  100  particles  was  executed  and  the  resulting  plot 
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is  shown  in  Fig.  13.  The  resulting  plots  for  the  other  two  cases  (500  and  2000  particles) 
are  shown  in  Figs.  14  and  15,  respectively. 
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Figure  13.  MSEs  for  the  non-Gaussian  noise  case  using  100  particles 

The  results  of  Fig.  13  indicate  extensive  improvement  for  the  PF.  The  average 
MSEs  were  computed  to  be  0.99227  for  the  measurement  and  0.67816  for  the  PF  esti¬ 
mate.  However,  in  a  few  simulations  the  PF  generated  large  errors.  This  fact  implies  the 
necessity  to  hold  to  the  initial  guideline  for  using  500  to  2000  particles  in  order  to  guaran¬ 
tee  estimates  better  than  the  measurements. 

Obviously,  both  implementations  for  500  and  2000  particles  seem  to  provide  good 
estimates  of  the  state.  The  improved  performance  of  the  PF  is  indicated  by  the  average 
MSEs  as  well.  All  of  the  computed  values  for  the  average  MSEs  for  both  Gaussian  and 
biased  non-Gaussian  noise  cases  regarding  the  PF  are  summarized  and  tabulated  in  Table 
2.  The  same  table  contains  the  computed  improvement  ratios  for  the  PF  as  presented  in 
Eq.  (62)  in  order  to  provide  a  general  measure  of  comparison  for  all  simulated  implemen¬ 
tations. 


MSE  distribution  for  PF  using  100  particles,  non  Gaussian  additive  noise 
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MSE  distribution  for  PF  using  100  particles,  non  Gaussian  additive  noise 


Figure  14.  MSEs  for  the  non-Gaussian  noise  case  using  500  particles 


MSE  distribution  for  PF  using  2000  particles,  non  Gaussian  additive  noise 


Figure  15.  MSEs  for  the  non  Gaussian  noise  case  using  2000  particles 
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Number  of 

Linear  model  in  Gaussian  noise 

Linear  model  in  non  Gaussian  noise 

particles  used 

Average 

MSEmeas 

Average 

MSEpf 

PF 

Average 

MSEmeas 

Average 

MSEpf 

Ipp 

10 

0.99848 

0.80476 

1.24718 

0.99083 

1.49760 

0.66161 

20 

0.98340 

0.68623 

1.43305 

- 

- 

- 

50 

- 

- 

- 

0.99605 

0.75424 

1.32060 

100 

0.99105 

0.61723 

1.60564 

0.99227 

0.67816 

1.46318 

500 

0.97850 

0.58812 

1.66378 

0.98803 

0.59702 

1.65494 

2000 

0.98517 

0.58977 

1.67043 

0.98525 

0.58488 

1.68453 

5000 

0.98403 

0.58945 

1.66940 

- 

- 

- 

Table  2.  MSE  comparison  chart  for  PF  performance  in  linear  model  implementations 


The  results  for  the  two  linear  model  cases  show  that  the  nature  of  the  additive 
noise  significantly  affects  the  performance  of  the  PF  when  an  insufficient  number  of  par¬ 
ticles  is  used.  On  the  contrary,  when  enough  particles  are  propagated  in  each  iteration,  the 
PF  yields  robust  performance  regardless  of  the  nature  of  the  noise. 

The  simulation  results  obtained  up  to  this  point  can  be  used  to  decide  on  the  num¬ 
ber  of  particles  to  be  used  in  the  attitude  determination  problem.  In  the  literature  [7]  it  is 
stated  that  a  PF  using  2000  particles  would  guarantee  convergence  for  this  specific  appli¬ 
cation.  This  statement  is  in  agreement  to  the  conclusions  that  have  been  derived  up  to  this 
point. 

C.  PF  EVALUATION  IN  ATTITUDE  DETERMINATION  FOR  THE 

UNBIASED  ONE-AXIS  ROTATION  CASE 

In  this  section,  the  SIR  algorithm  developed  in  Chapter  II  is  tested  for  attitude  de¬ 
termination  based  on  the  model  described  by  the  set  of  Eqs.  (57)  and  (58).  The  simulation 
lasts  for  20  seconds  using  a  discretization  time  interval  of  0.01  seconds  resulting  to  a  total 
of  2000  time  samples,  assuming  zero  initial  conditions.  The  disturbance  on  the  system  is 
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added  in  the  form  of  noise  in  the  angular  rate  measurements.  Despite  the  use  of  the  qua¬ 
ternion  representation  for  the  system  modeling,  the  final  evaluation  is  based  on  the  error 
reduction  in  the  roll  angle  using  the  quaternion  estimation,  due  to  the  fact  that,  in  prac¬ 
tice,  the  Euler  angle  estimation  is  of  interest. 

1.  Data  Generation 

The  data  are  generated  assuming  a  sinusoidal  angular  rate  disturbance  around  the 
x-axis  of  the  form: 

co{t)  =  Asm{(O0t)  (63) 

where  00(t)  is  the  angular  rate  value  at  time  t,  and  the  parameters  A  and  (O0  are  the  ampli¬ 
tude  and  the  natural  frequency,  respectively.  Figure  16  shows  the  generated  true  values  of 
the  angular  rate  through  time. 


True  values  of  co 


Figure  16.  True  values  of  angular  rate 
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The  parameters  A  and  (O0  were  selected  in  such  a  way  that  the  resulting  roll  angle 
representation  included  discontinuities  during  the  simulation  interval.  The  specific  values 
used  were  A  =  1  rad/sec  and  G)0  =  0.1  rad/sec. 

The  noise  sequences  n[k]  and  v[&]  presented  in  the  system  model  were  assumed 
to  be  white  Gaussian  noise  with  standard  deviations  of  <Jn  =  1 0  rad/sec  and  <JY  =  10  de¬ 
grees  respectively.  These  values  were  selected  in  order  to  clearly  visualize  the  additive 
noise  in  the  associated  plots.  The  resulting  true  and  measured  roll  angles  sequences  are 
shown  in  Fig.  17. 


True  value  of  roll  angle 


Figure  17.  True  and  measured  values  of  the  roll  angle 


It  is  obvious  that  there  are  two  discontinuities  in  the  roll  angle  sequence  that  have 
to  be  overcome  by  the  estimator.  Due  to  these  discontinuities,  an  implementation  using 
the  EKF  can  not  provide  satisfactory  results  and  the  PF  is  a  better  approach. 
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2.  Attitude  Representation 

In  order  to  proceed  to  the  estimation  step,  the  required  quantities  have  to  be  com¬ 
puted.  First  of  all,  the  roll  angle  measurement  set  is  converted  to  the  associated  normal¬ 
ized  quaternion  set.  The  actual  quaternion  values  and  the  ones  calculated  by  the  noisy 
measurement  set  are  shown  in  Fig.  18. 


True  quaternion  values 


Figure  18.  True  and  calculated  from  noisy  measurements  quaternion  values 

Since  the  dynamic  model  derived  in  the  previous  chapter  is  expressed  in  terms  of 
the  quaternion  component  qx ,  this  is  the  state  to  be  estimated.  The  task  to  be  accom¬ 
plished  is  the  minimization  of  the  state  error  which  is  presented  in  Fig.  19.  It  is  noticeable 
that  the  state  error  is  reduced  around  the  time  indices  that  the  quaternion  receives  its 
boundary  values  ±1 .  This  is  due  to  the  normalization  constraint  enforcement  that  pre¬ 
vents  the  state  from  receiving  invalid  values.  The  figure  also  contains  the  measurement 
error. 


43 


a.  -40  I _ 1 _ I _ I _ I _ I _ I _ I _ I _ I _ 

0  2  4  6  8  10  12  14  16  18  20 

Time  (sec) 


Figure  19.  Quaternion  and  Euler  angle  error 

3.  Estimation  Analysis 

For  the  state  estimation  the  PF  approach  and  the  dynamic  model  derived  in  Chap¬ 
ter  III  were  used.  The  generated  noisy  data  set  for  the  roll  angle  was  given  as  an  input  to 
the  estimator.  Two  different  implementations  using  500  and  2000  particles  were  tested. 
No  initial  estimation  error  was  assumed  for  any  of  the  simulations  and  the  performance 
was  compared  in  terms  of  the  MMSE  criterion.  The  resulting  estimate  qx ,  of  the  quater¬ 
nion  component  ql ,  was  converted  to  the  associated  roll  angle  generating  the  final  esti¬ 
mate  (p .  The  estimation  error,  eest  =  \<ptme  -  (p ||,  was  compared  to  the  measurement  error 

^meas  \  \  ^P true  ^P meets  \  \  ' 

The  simulation  indicated  that  the  developed  PF  is  capable  of  reducing  the  noise  to 
an  acceptable  level.  The  results  are  presented  in  the  following  sections. 
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a.  Estimation  using  500  particles 

The  resulting  curve  of  the  estimated  state  is  presented  in  Fig.  20.  Two  ran¬ 
dom  parts  of  the  figure  have  been  scaled-up  to  provide  better  visualization  of  the  estima¬ 
tion  error.  Obviously,  there  has  been  a  significant  reduction  of  the  state  error  and  the  es¬ 
timation  algorithm  is  shown  to  be  effective. 


True  versus  estimated  value  of  q1 
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Figure  20.  State  estimation  using  500  particles 


The  most  crucial  point  for  the  filter  evaluation  is  the  analysis  of  the  esti¬ 
mated  roll  angle  curve  which  is  presented  in  Fig.  2 1 .  The  estimated  curve  is  smooth  and 
approximately  overlaid  over  the  actual  one  indicating  small  estimation  error.  Two  ran¬ 
dom  parts  of  the  figure  have  been  scaled-up  to  provide  better  visualization.  The  evalua¬ 
tion  of  the  PF  performance  is  done  using  the  MSE  criterion.  The  MSE  for  the  available 

set  of  measurements  was  (10.0255  deg)"  while  the  MSE  of  the  filtered  estimate  was 
(0.64283  deg)" .  This  means  that,  in  terms  of  MSE,  the  PF  provided  an  improvement  in 
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Absolute  error  in  degrees  Roll  angle  in  degrees  Roll  angle  in  degrees 


the  error  reduction  by  a  factor  of  243.23.  Finally,  both  measurement  and  estimation  abso¬ 
lute  errors  are  presented  in  Fig.  22  in  semi-logarithmic  form. 


True  versus  estimated  value  of  4> 
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Figure  2 1 .  Roll  angle  estimation  using  500  particles 


Time  (sec) 


Figure  22.  Comparison  of  the  measurement  and  estimation  errors  using  500  particles 
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b.  Estimation  Using  2000  Particles 

For  this  simulation  the  same  set  of  noisy  measurements  was  used.  The  in¬ 
creased  number  of  particles  provided  even  better  performance.  The  same  set  of  plots  was 
generated  and  the  Figs.  23,  24  and  25  show  the  resulting  state  estimation,  the  roll  angle 
estimation  and  the  absolute  error  representations,  respectively. 


True  versus  estimated  value  of  q1 
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Figure  23.  State  estimation  using  2000  particles 


As  expected,  the  state  error  is  reduced  in  comparison  with  the  500  parti¬ 
cles  case.  As  a  result,  the  roll  angle  estimation  error  is  also  reduced  as  it  can  be  clearly 
noticed  by  Figs.  24  and  25.  The  resulting  MSE  for  the  roll  angle  estimate  in  this  case  is 

(0.40982  deg)“ ,  resulting  in  a  factor  of  approximately  598  concerning  the  error  reduction 

rate  in  terms  of  the  MSE.  Finally,  all  the  results  for  both  implementations  are  summarized 
and  tabulated  in  Table  3. 
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True  versus  the  estimated  from  the  filter  output  value  of  ^ 
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Figure  24.  Roll  angle  estimation  using  2000  particles 
Measurement  and  estimation  errors 


Figure  25.  Comparison  of  the  measurement  and  estimation  errors 

using  2000  particles 
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Number  of 

particles  used 

^ meas 

(deg) 

MSEmeas 

(deg2) 

eest 

(deg) 

MSEest 

(deg2) 

mse/ 

/MSEest 

500 

8.0533 

100.51 

0.60591 

0.41323 

243.23 

2000 

8.0533 

100.51 

0.31663 

0.16795 

598.45 

Table  3.  MSE  comparison  chart  for  P 

7  performance  in  the  nonlinear  application 

D.  SUMMARY 

In  this  chapter,  the  PF  (based  on  the  developed  SIR  algorithm)  was  tested  on  vari¬ 
ous  cases.  Particular  emphasis  was  given  to  the  nonlinear  application  of  attitude  determi¬ 
nation  for  the  NPS  TASS  for  the  unbiased  one-axis  rotation  case. 

At  first,  the  good  performance  of  the  PF  was  verified  for  the  linear  model  regard¬ 
less  the  statistics  of  the  disturbances.  The  major  impact  of  the  number  of  propagated  par¬ 
ticles  in  determining  the  accuracy  of  the  estimation  was  underlined.  On  the  other  hand, 
the  computational  intensity  is  a  constraint  that  has  to  be  taken  under  consideration  with 
respect  to  any  particular  application. 

Finally,  the  PF  demonstrated  excellent  performance  in  the  nonlinear  model  esti¬ 
mation  problem.  Both  implementations  that  were  tested  provided  high  accuracy  results. 

The  following  chapter  of  this  thesis  provides  the  conclusions  and  suggests  direc¬ 
tions  for  future  expansion  of  the  research. 
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V.  CONCLUSIONS 


The  PF  based  on  the  SIR  algorithm  can  be  utilized  as  an  accurate  estimator  of 
spacecraft  attitude  using  quaternion  representation.  It  demonstrates  robust  performance 
even  when  implemented  in  a  nonlinear  dynamic  model  overcoming  the  difficulties  intro¬ 
duced  by  discontinuities. 

A.  SUMMARY 

A  PF  estimator  for  attitude  angular  determination  has  been  presented  in  this  the¬ 
sis.  Unlike  standard  implementations  using  the  KF,  the  approach  presented  is  completely 
general  and  can  be  adapted  to  any  nonlinear,  non-Gaussian  statistical  model,  without  the 
need  of  any  linearization.  In  particular,  the  implementation  to  one-axis  rotation  has 
shown  the  effectiveness  even  in  the  presence  of  discontinuities  in  the  model. 

B.  RECOMMENDATIONS 

It  is  recommended  that  future  work  include  the  following  expansions  of  this  re¬ 
search: 

1.  Expansion  to  the  Biased  Three-axis  Rotation  Case 

The  dynamic  model  for  spacecraft  attitude  determination  for  the  biased  three-axis 
rotation  scenario  has  been  derived  in  this  thesis.  The  PF  implementation  has  to  be  ex¬ 
panded  to  include  the  7-dimensional  state  vector  and  some  simulations  have  to  be  exe¬ 
cuted.  This  research  will  provide  much  more  realistic  results  regarding  the  final  utiliza¬ 
tion  of  PF  in  this  specific  application. 

2.  Gibbs  Vector  Parameterization 

Although  in  this  thesis  the  quaternion  representation  was  used  for  all  rotations,  a 
better  approach  would  be  to  represent  small  angular  updates  with  the  Gibbs  vector  and 
then  project  back  to  quaternion.  The  reason  behind  this  logic  is  the  fact  that  Gibbs  vectors 
are  additive  and  do  not  need  normalization,  while  quaternions  need  to  be  nonnalized  at 
every  step,  thus  introducing  possible  biased  estimates. 
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APPENDIX  A:  SCATTER  PLOTS  FOR  THE  MSE  IN  THE  LINEAR 

MODEL 


A.  GAUSSIAN  ADDITIVE  NOISE  CASE 

The  following  figures  are  associated  with  the  results  presented  by  the  histograms 
in  Figs.  5  through  10  of  the  thesis,  for  10,  20,  100,  500,  2000  and  5000  particles.  The  ex¬ 
act  value  of  the  MSE  is  plotted  as  a  single  dot  for  every  simulation  and  the  comparison  of 
the  performance  of  the  two  filters  can  be  visualized  by  the  vertical  distance  of  the  related 
dots  in  the  plot.  It  is  clearly  obvious  that,  when  the  number  of  particles  is  increased,  the 
distance  between  the  red  (PF  MSE)  and  the  blue  (KF  MSE)  dots  is  reduced  significantly 
verifying  the  theoretical  approach  that  the  PF  converges  to  the  KF  when  infinite  number 
of  particles  is  used. 


MSEs  for  a  simulation  of  1000  runs,  10  particles  used 
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MSEs  for  a  simulation  of  1000  runs,  20  particles  used 
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MSE  MSE 


MSEs  for  a  simulation  of  1000  runs,  500  particles  used 
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MSEs  for  a  simulation  of  1000  runs,  2000  particles  used 
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MSEs  for  a  simulation  of  1000  runs,  5000  particles  used 
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ercentage  of  better  Kalman  filter  performance:  66.1 
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B.  NON-GAUSSIAN  ADDITIVE  NOISE  CASE 

The  following  figures  are  associated  directly  to  the  results  presented  by  the  histo¬ 
grams  in  Figs.  1 1  through  15  of  the  thesis  for  10,  50,  100,  500  and  2000  particles.  The 
same  mapping  as  the  previous  section  of  the  appendix  is  used.  With  these  plots  it  is  quite 
obvious  that  in  a  non-Gaussian  environment,  an  insufficient  number  of  particles  may  re¬ 
sult  in  a  probable  nonconvergence  for  the  PF.  This  can  be  shown  by  the  large  values  of 
MSE  that  are  noticeable  in  a  number  of  simulations.  As  the  number  of  particles  increases, 
the  phenomenon  is  reduced  and  finally  eliminated  in  the  2000-particle  case. 
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MSEs  for  a  simulation  of  1000  runs,  100  particles  used 
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MSEs  for  a  simulation  of  1000  runs,  2000  particles  used 
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APPENDIX  B:  MATLAB  CODE  USED  FOR  THE  NONLINEAR 

MODEL  APPLICATION 


This  following  section  includes  the  code  that  was  used  with  the  Mat  lab"  software 
to  perfonn  the  simulations  for  the  nonlinear  application.  The  code  consists  of  the  main 
module  (called  SIRnonlinear.m),  the  functions  called  through  it  (SIRfun.m,  predict.m, 
weightvector.m  and  resample. m)  and  the  Simulink®  model  (called  quat2euler.mdl)  which 
provides  accurate  conversion  of  the  quaternions  to  Euler  angles.  Each  one  of  the  Matlab  ® 
files  is  accompanied  by  the  appropriate  comments  for  further  reference.  Some  basic  ideas 
for  the  functions  SIR  fun.m,  predict.m,  weightvector.m  and  resample. m  were  found  in 
Ref.  [15]. 

A.  MAIN  MODULE 

%  LT  IOANNIS  KASSALIAS 

%  DATA  GENERATION  AND  FILTERING  ALGORITHM  FOR  THE 
NONLINEAR  PROBLEM 

%  Generate  the  true  values  of  angular  rates  given  the  model 
%  omega=A*sin(omega_0*t+phi)  for  the  1  dimensional  case 
%  Calculate  the  true  values  of  quaternions  and  Euler  angles 
%  Generate  noise  sequences  for  both  angular  rate  and  Euler  angle 
%  measurement 

%  Calculate  the  related  "noisy"  values  of  the  quaternion 
%  Perform  SIR  filtering  and  plot  the  results 
clc 
clear 

format  short  g 
tic 

%  Define  parameters 
A=10A0; 
omega_0=0.1; 
values 

%  phi=[0. 1,0.2, 0.3]; 
lar  rate 
phi=[0  0  0]; 
sigma=10A-1; 
gular  rate 
sigma2=10A1; 

Euler  angles 
q0=[0;0;0;1]; 

Dt=0.01 ; 
t_final=20; 


%  Max  amplitude  of  the  angular  rate 

%  Frequency  used  to  generate  angular  rate 

%  Used  as  test  for  the  initialization  of  the  angu- 

%  Initial  phase  for  the  angular  rate  value 

%  Standard  deviation  for  the  noise  added  to  an- 

%  Standard  deviation  for  the  noise  added  to 

%  Quaternion  initial  condition 
%  Sampling  interval 
%  Simulation  duration 
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%  Generate  data  for  angular  rates 
t=Dt:Dt:t_final;  %  Time  vector 

for  ii=1 :3 
if  ii==1 

omega_true(ii,:)=A*sin(omega_0*t+phi(ii));  %  True  value  of  omega 
else 

omega_true(ii,:)=zeros(1,t_final/Dt); 

end 

end 

plot(t,omega_true(1,:),t,omega_true(2,:),'r',t,omega_true(3,:),'g'),grid 

title('T rue  values  of  \omega') 

xlabel('Time  (sec)') 

ylabel('\omega_t_r_u_e  (rad/sec)') 

legend('\omega_1','\omega_2','\omega_3',0) 

%  Generate  OMEGA  matrices  of  angular  rates 
for  n=1 :1:t_final/Dt 

omega_true_cross(:,:,n)=[0  -omega_true(3,n)  omega_true(2,n)  ; 
omega_true(3,n)  0  -omega_true(1,n)  ;  -omega_true(2,n)  omega_true(1  ,n) 
0]; 

OMEGA(:,:,n)=[-omega_true_cross(:,:,n)  omega_true(:,n)  ; 
(omega_true(:,n)')  0]; 
end 

%  Compute  quaternions 

%  Solve  differential  equation  q_dot=0.5*OMEGA*q*Dt 
%  treating  it  as  a  difference  equation  in  discrete  time 
%  and  considering  a  time  interval  Dt=0.01  sec 
q_true(:,:,1)=q0;  %  Set  initial  condition 

for  n=2:1:t_final/Dt 

q_true(:,:,n)=Dt*0.5*OMEGA(:,:,n-1)*q_true(:,:,n-1)+q_true(:,:,n-1); 
q_true(:,:,n)=q_true(:,:,n)/norm(q_true(:,:,n));  %  Normalize 

quaternions 
end 

%  Plot  the  quaternions 

q_1  =reshape(q_true(  1 , 1 , : ),  1  ,t_final/Dt); 

q_2=reshape(q_true(2,1,:),1,t_final/Dt); 

q_3=reshape(q_true(3 , 1 , : ),  1  ,t_final/Dt); 

q_0=reshape(q_true(4 , 1 ,:),  1  ,t_final/Dt); 

figure 

plot(t,q_1,t,q_2,'r',t,q_3,'g',t,q_0,'m'),grid 
title('Plot  of  the  quaternions  vs.  time') 
xlabel('Time  (sec)') 

ylabel('Quaternion  values  (normalized)') 
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Iegend(,q_1,,,q_2,,,q_3',,q_0',0) 

%  Detect  the  zero  crossings  for  quaternions 
for  n=2:1:t_final/Dt 

if  sign(q_1  (n+1  ))~=sign(q_1  (n)) 
tt1=n+1; 
break 
end 
end 

for  n=tt1:1:t_final/Dt 

if  sign(q_1  (n+1  ))~=sign(q_1  (n)) 
tt2=n+1; 
break 
end 
end 

for  n=2:1:t_final/Dt 

if  sign(q_0(n+1  ))~=sign(q_0(n)) 
tt3=n+1; 
break 
end 
end 

for  n=tt3:1  :t_final/Dt 

if  sign(q_0(n+1  ))~=sign(q_0(n)) 
tt4=n; 
break 
end 
end 

%  Generate  sign  sequences  in  order  to  solve  correctly  the  normalization 
%  constraint  equation  q1=+-sqrt(1-qOA2) 
flag=ones(1,2000); 
for  i=tt3:tt4 
flag(i)=-flag(i); 
end 

flag2=ones(1,2000); 
for  i=tt1  :tt2 

flag2(i)=-flag2(i); 

end 

%  Conversion  of  quaternions  to  Euler  angles 

E_true=[]; 

for  n=1:1:t_final/Dt 

q_true_0=q_true(1,1,n);  q_true_1=q_true(2,1  ,n); 

q_true_2=q_true(3,1  ,n);  q_true_3=q_true(4,1  ,n); 
quaternion=[q_true_0  q_true_1  q_true_2  q_true_3]; 
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sim('Quat2Euler') 

E_true=[E_true,eul']; 

end 

figure 

plot(t,E_true(1  ,:),t,E_true(2,:),'r',t,E_true(3,:),,g,),grid 
title('T rue  values  of  Euler  angles  versus  time') 
xlabel('Time  (sec)') 
ylabel('Euler  angles  (degrees)') 

legend('Roll  angle  \phi', 'Pitch  angle  \theta','Yaw  angle  \psi',0) 
E1=E_true(1,:); 

%  Generate  measured  value  of  angular  rate  sequences 
omega_m=[]; 
for  ii=1 :3 
if  ii==1 

n_w=sigma*randn(1  ,t_final/Dt);  %  Generate  noise  sequence 

omega_m(ii,:)=omega_true(ii,:)+n_w; 
else 

omega_m(ii,:)=omega_true(ii,:); 

end 

end 

%  Plot  the  true  versus  the  measured  values  of  angular  rates 
figure 

subplot(31 1) 

plot(t,omega_true(1,:),'LineWidth',1),hold  on 
plot(t,omega_m(1,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\omega_1  (rad/sec)') 
legend('True  value', 'Noisy  measurement', 0) 
title('T rue  versus  measured  values  of  angular  rate') 
subplot(312) 

plot(t,omega_true(2,:),'LineWidth',1  ),hold  on 
plot(t,omega_m(2,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\omega_2  (rad/sec)') 
legend('True  value', 'Noisy  measurement', 0) 
subplot(313) 

plot(t,omega_true(3,:),'LineWidth',1  ),hold  on 
plot(t,omega_m(3,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\omega_3  (rad/sec)') 
legend('True  value', 'Noisy  measurement', 0) 

%  Add  noise  to  the  Euler  angles  to  generate  measurement  sequences 
for  ii=1 :3 
if  ii==1 

n_E=sigma2*randn(1  ,t_final/Dt);  %  Generate  noise  sequence 
E_m(ii,:)=E_true(ii,:)+n_E; 
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else 

E_m(ii,  :)=E_true(ii, :); 
end 
end 

%  Plot  the  true  versus  the  measured  values  of  Euler  angles 
figure 

subplot(31 1) 

plot(t,E_true(1  ,:),'LineWidth',3),hold  on 
plot(t,E_m(1  ,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\phi  (degrees)') 
legend('True  value', 'Noisy  measurement', 0) 
title('T rue  versus  measured  values  of  Euler  angles') 
subplot(312) 

plot(t,E_true(2,:),'LineWidth',3),hold  on 
plot(t,E_m(2,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\theta  angle  (degrees))') 
legend('True  value', 'Noisy  measurement', 0) 
subplot(313) 

plot(t,E_true(3,:),'LineWidth',3),hold  on 
plot(t,E_m(3,:),'r:'), grid, hold  off 
xlabel('Time  (sec)'),ylabel('\psi  (degrees)') 
legend('True  value', 'Noisy  measurement', 0) 

%  Conversion  back  to  quaternions 

E_m_rad=E_m*pi/180; 

c1=cos(E_m_rad(1,:)/2); 

s1=sin(E_m_rad(1,:)/2); 

c2=cos(E_m_rad(2,:)/2); 

s2=sin(E_m_rad(2,:)/2); 

c3=cos(E_m_rad(3,:)/2); 

s3=sin(E_m_rad(3,:)/2); 

c1c2=c1.*c2; 

s1s2=s1  ,*s2; 

q_0_m = (c  1  c2 .  *c3-s  1  s2 .  *s3 ) .  *f  I  ag ; 
q_1_m=(s1  ,*c2.*c3+c1  ,*s2.*s3).*flag; 
q_2_m=c1c2.*s3+s1s2.*c3; 
q_3_m=c1  ,*s2.*c3-s1  ,*c2.*s3; 
figure 

subplot(221 ) 

plot(t,q_0,t,q_0_m,'r'),xlabel('Time  (sec)') 

title('q_0  true  versus  calculated  from  noisy  Euler  angles  measurements 

values') 

subplot(222) 

plot(t,q_1  ,t,q_1_m,'r'),xlabel('Time  (sec)') 
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title('q_1  true  versus  calculated  from  noisy  Euler  angles  measurements 

values') 

subplot(223) 

plot(t,q_2,t,q_2_m,'r'),xlabel('Time  (sec)') 

title('q_2  true  versus  calculated  from  noisy  Euler  angles  measurements 

values') 

subplot(224) 

plot(t,q_3,t,q_3_m,'r'),xlabel('Time  (sec)') 

title('q_3  true  versus  calculated  from  noisy  Euler  angles  measurements 

values') 

t1=t; 


%%%  FILTERING  STAGE  %%% 

%  Define  parameters 

N  =  2000;  %  Number  of  iterations 

t  =  1 : 1  :N;  %  Time  step  index 

R  =  sigma2A2;  %  Measurement  noise  variance 

Q  =  sigmaA2;  %  Angular  rate  noise  variance 

numSamples  =  2000;  %  Number  of  particles  used  in  each  iteration 

%  Generate  error  sequences 
v  =  E_true(1,:)-E_m(1,:); 
w  =  q_1-q_1_m; 
figure 

subplot(21 1 ) 

plot(t1  ,w),grid,xlabel('Time') 
ylabel('Quaternion  q_1  error') 
subplot(212) 

plot(t1  ,v),grid,xlabel('Time') 
ylabel('Roll  angle  error  in  degrees') 

%  Determine  state  and  measurement 

x=q_1_m';  %  State  sequence  calculated  by  noisy  angular  rate 

measurement 

y=E_m(1 , :)'*pi/1 80;  %  Noisy  roll  angle  measurement 

%  Perform  SIR  filtering 

[samples, q]  =  SIR_fun(y,R,Q,  numSamples,  tt3,tt4); 

%  Compute  MMSE  estimate 
q_1_est  =  mean(samples); 

%  Generate  estimate  of  the  scalar  value  quaternion 
q_1_est=flag2.*abs(q_1_est); 
q_0_est=flag.*abs(sqrt(1-q_1_est  A2)); 
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%  Plot  filtered  output  for  the  quaternion 
figure 

subplot(21 1) 

plot(t1,q_1,t1,q_1_est,'r'),xlabel(Time'),ylabel('Quaternion  value  q_1'),grid 
legend('True  value  of  q_1  '/Estimated  value  of  q_1',0) 

title('T rue  versus  estimated  value  of  q _ 1 ') 

subplot(212) 

plot(t1  ,q_1  ,t1  ,q_1_m,'r'),xlabel('Time  (sec)'),ylabel('Quaternion  value 
q_1'),grid 

legend('True  value  of  q _ 1 'Value  estimated  by  the  noisy  Euler  angle 

measurement') 

title('q_1  true  versus  calculated  from  noisy  Euler  angles  measurements 

values') 

figure 

error_meas=abs(q_1  -q_1_m); 
error_filter=abs(q_1  -q_1_est); 
semilogy(t1  ,error_meas,t1  ,error_filter,'r') 
xlabel('Time'),ylabel('Absolute  error'), grid 

legend('Error  produced  by  noisy  measurement', 'Error  of  the  filtered  out¬ 
put', 0) 

title('Error  representation  in  terms  of  quaternion  value  q _ 1 ') 

err_rate_quat=mean(error_meas)/mean(error_filter) 

%  Conversion  to  Euler  angles 
E_est=[];  clear  eul 
for  n=1:1:t_final/Dt 

quaternion=[q_1_est(n)  0  0  q_0_est(n)]; 
sim('Quat2Euler') 

E_est=[E_est,eul']; 

end 

E2=E_est(1,:); 

%  Plot  the  true  and  the  estimated  values  of  the  roll  angle 
figure 

plot(t1  ,E1  ,t1  ,E2,'r'),grid 

xlabel('Time'),ylabel('Roll  angle  in  degrees') 

legend('True  value  of  roll  angle', 'Estimated  value  of  roll  angle', 0) 

title('T rue  versus  the  estimated  from  the  filter  output  value  of  roll  angle') 

%  Generate  and  plot  the  error  curves 

err_m_ang=abs(E1-E_m(1,:)); 

err_est_ang=abs(E1  -E2); 

semilogy(t1  ,err_m_ang,t1  ,err_est_ang,'r'),grid 

xlabel('Time'),ylabel('Absolute  error  in  degrees') 

legend('lnitial  error  due  to  measurement', 'Error  of  the  filtered  output', 0) 
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error_rate_angle=mean(err_m_ang)/mean(err_est_ang) 

MSE_est=sum(err_est_ang.A2)/length(err_est_ang) 

MSE_meas=sum(err_m_ang.A2)/length(err_m_ang) 

toe 

B.  FUNCTION  SIR  FUN.M 

function  [x,q]  =  SIR_fun(y,R,Q,numSamples,tt3,tt4) 

%  This  function  is  used  to  perform  SIR  filtering  for  the  specified 
%  nonlinear  model.  The  given  inputs  are: 

%  y  -->  The  available  set  of  measurements 
%  R  -->  The  measurement  noise  variance 
%  Q  -->  The  process  noise  variance 
%  numSamples  -->  The  number  of  particles 
%  tt3  -->  The  first  zero  crossing  of  quaternion  qO 
%  tt4  — >  The  second  zero  crossing  of  quaternion  qO 
%  The  provided  outputs  are: 

%  x  -->  The  estimated  state  trajectories 
%  q  ~>  The  normalized  weighting  vector 

if  nargin  <  6,  error('Not  enough  input  arguments.');  end 

[rows, cols]  =  size(y);  %  rows  =  Number  of  iterations 

x=zeros(numSamples,rows);  %  Initialize  posterior  mean  estimate 

xu=zeros(numSamples,rows);  %  Initialize  predicted  state 

q=zeros(numSamples,rows);  %  Initialize  weighting  vector 

%  Performing  prediction  and  updating  after  resampling 
for  t=1  :rows-1 

xu(:,t)  =  predict(x(:,t),t,Q,tt3,tt4); 

q(:,t+1)  =  weightvector(xu(:,t),y(t+1,1),R,tt3,tt4); 

x(:,t+1)  =  resample(xu(:,t),q(:,t+1)); 

x(:,t+1)  =  xu(:,t); 

[q(:,t+1  ),xu(:,t),x(:,t+1 )]; 
end 

C.  FUNCTION  PREDICT.M 
function  xu  =  predict(x,t,Q,tt3,tt4) 

%  This  function  provides  the  results  of  the  prediction  stage  of  the  SIR 
%  based  particle  filter.  The  inputs  are: 

%  x  ->  The  generated  samples  of  the  state 
%  t  -->  The  iteration  index  number 
%  Q  -->  The  variance  of  the  process 
%  tt3  -->  The  first  zero  crossing  of  quaternion  qO 
%  tt4  -->  The  second  zero  crossing  of  quaternion  qO 

if  nargin  <  5,  error('Not  enough  input  arguments.');  end 

w  =  sqrt(Q)*randn(size(x)); 
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if  (t>=tt3)&&(t<=tt4) 

xu  =  -0.5.*(sin(0.1*t*0.01  )+w).*sqrt(1-x.A2)*0.01  +  x; 
else 

xu  =  0.5.*(sin(0.1*t*0.01  )+w).*sqrt(1-x.A2)*0.01+  x; 
end 

D.  FUNCTION  WEIGHTVECTOR.M 

function  q  =  weightvector(xu,y,R,tt3,tt4) 

%  This  function  computes  the  normalized  weighting  vector  q 
%  The  given  inputs  are: 

%  xu  -->  The  set  of  state  prediction  samples 
%  y  -->  The  available  set  of  measurements 
%  R  -->  The  measurement  noise  covariance 
%  tt3  -->  The  first  zero  crossing  of  quaternion  qO 
%  tt4  — >  The  second  zero  crossing  of  quaternion  qO 
%  NOTE:  The  equation  for  variable  m  is  the  measurement  equation 
%  for  the  one-axis  case 

if  nargin  <  5,  error('Not  enough  input  arguments.');  end 

flag=ones(1,2000); 
for  i=tt3:tt4 
flag(i)=-flag(i); 
end 

[rows, cols]  =  size(xu); 
q  =  zeros(size(xu)); 

m  =  atan2(2*xu.*sqrt(1-xu.A2) ,  1-2*xu.A2); 
q  =  exp(-.5*RA(-1)*(y.*ones(size(xu))-m)  A(2)); 
q  =  q/sum(q); 

E.  FUNCTION  RESAMPLE.M 
function  x  =  resample(xu,q) 

%  This  function  performs  the  resampling  stage  providing  the  new  set  of 
%  the  particles  at  the  end  of  each  iteration.  The  inputs  are: 

%  xu  -->  The  set  of  state  prediction  samples 
%  q  -->  The  normalized  weighting  vector 

if  nargin  <  2,  error('Not  enough  input  arguments.');  end 

i  =  1; 
j  =  1; 

[N,M]=size(xu); 
u  =  rand(N+1 ,1); 
x  =  ones(size(xu)); 

S  =  cumsum(-log(u)); 

W  =  cumsum(q); 
while  j  <=  N 

if  (W(j,  1  )*S(N,  1 ))  >  S(i,1) 
x(i,1)  =  xu(j,1 ); 
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i  =  i+1; 
else 

j  =  j+i; 

end 

end 

F.  SIMULINK®  MODEL  QUAT2EULER.MDL 
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